package owg.util.opengl;

import java.util.Iterator;
import java.util.LinkedList;

import owg.util.Calc;


/**
 * An armature bone
 */
public class Bone
	{
	public static final byte PM_OFF = 0;
	public static final byte PM_EARLY = 1;
	public static final byte PM_LATE = 2;
	
	static final float toRad = (float) (Math.PI/180);
	private Bone parent;
	public Bone mirrorBone;
	public Armature armature;
	public String name;
	public int index;
	public float[] undeformedPosition;//The original position on the undeformed mesh
	public float[] offsetFromParent;//The undeformed offset from the parent bone
	public float[] currentPosition;//The current position in armature space
	/**Whether to multiply the units matrix with the parent's unit matrix*/
	public boolean relativeUnits;
	/**Whether the position should be mirrored when setting a mirrored pose*/
	public boolean mirrorPosition;
	
	//Rotation data:
	float[] data;
	//Which channels the data contain
	boolean posData;
	boolean rotData;
	boolean scaData;//not impl
	
	public float scale;
	
	public float[][] units;
	float[][] oldUnits;
	/**Units is multiplied with this matrix during compute, if usePostMatrix is true*/
	public float[][] postMatrix;
	public byte postMatrixMode;
	/**This offset is added to the position in world space, if usePostOffset is true*/
	public float[] postOffset;
	public boolean usePostOffset;
	
	private int blockSize;
	private int currentDataPosition;
	/**A list of children. May be used for recursive computing, but is normally not useful since
	 * a topologically sorted array is in the armature object.*/
	private LinkedList<Bone> children = new LinkedList<Bone>();
	
	/**If true, interpolation is disabled for this bone.*/
	private boolean lockInterp;
	
	/**When non-null, this is an end bone with the indicated undeformed length and direction.*/
	public float[] endSite;
	
	public Bone(Armature armature, Bone parent, String name, int index)
		{
		this.armature=armature;
		if(parent!=null)
			setParent(parent);
		this.name=name;
		this.index=index;
		this.blockSize=0;
		this.units = new float[][]{{1,0,0},{0,1,0},{0,0,1}};
		this.oldUnits = new float[][]{{1,0,0},{0,1,0},{0,0,1}};
		this.postMatrix = new float[][]{{1,0,0},{0,1,0},{0,0,1}};
		this.postMatrixMode = PM_OFF;
		this.postOffset = new float[]{0,0,0};
		this.usePostOffset = false;
		this.offsetFromParent= new float[]{0,0,0};
		this.currentPosition= new float[]{0,0,0};
		this.posData = false;
		this.rotData = false;
		this.scaData = false;
		this.data = null;
		this.scale = 1;
		this.relativeUnits = true;
		this.mirrorPosition = true;
		this.lockInterp = false;
		this.endSite = null;
		}
	/**Soft copy constructor. Parent must be set with setParent later on.
	 * Postmatrix and postOffset will be reset.*/
	public Bone(Armature armature, Bone dataSrc)
		{ 
		mirrorBone=null;//To be computed
		this.armature = armature;
		name = dataSrc.name;
		index = dataSrc.index;
		lockInterp = dataSrc.lockInterp;
		undeformedPosition = dataSrc.undeformedPosition.clone();
		offsetFromParent = dataSrc.offsetFromParent.clone();
		currentPosition = dataSrc.currentPosition.clone();
		relativeUnits = dataSrc.relativeUnits;
		mirrorPosition = dataSrc.mirrorPosition;
		data = dataSrc.data;
		posData = dataSrc.posData;
		rotData = dataSrc.rotData;
		scaData = dataSrc.scaData;
		scale = dataSrc.scale;
		units = Calc.clone2D(dataSrc.units);
		oldUnits = Calc.clone2D(units);
		endSite = dataSrc.endSite==null?null:dataSrc.endSite.clone();
		
		/*postMatrix = Calc.clone2D(dataSrc.postMatrix);
		usePostMatrix = dataSrc.usePostMatrix;
		postOffset = dataSrc.postOffset;
		usePostOffset = dataSrc.usePostOffset;*/

		this.postMatrix = new float[][]{{1,0,0},{0,1,0},{0,0,1}};
		this.postMatrixMode = PM_OFF;
		this.postOffset = new float[]{0,0,0};
		this.usePostOffset = false;
	
		blockSize = dataSrc.blockSize;
		currentDataPosition = dataSrc.currentDataPosition;
		}
	public void setOffsetFromParent(float[] offset)
		{
		if (parent==null)
			undeformedPosition = offset;
		else
			undeformedPosition = Calc.add(parent.undeformedPosition, 1, offset, 1);
		offsetFromParent = offset;
		}
	public void enablePosData()
		{
		if(!posData)
			blockSize+=3;
		posData = true;
		}
	public void enableRotData()
		{
		if(!rotData)
			blockSize+=3;
		rotData = true;
		}
	public void enableScaData()
		{
		if(!scaData)
			blockSize+=3;
		scaData = true;
		}
	
	/**
	 * This method multiplies the openGL transformation matrix(16 elements) with the current transformation around this bone.
	 * @see Bone#getPositionFor(float[])
	 * @param refPosition The position which you want to transform on the undeformed mesh. 
	 * Note that this position is in the undeformed space of the armature.
	 * @param workMatrix Matrix used to store the data. If null, a temporary matrix will be allocated.
	 */
	public void transformTo(EGL<?> gl, float[] refPosition, float[] workMatrix)
		{
		float[] v = new float[]{refPosition[0]-undeformedPosition[0],refPosition[1]-undeformedPosition[1],refPosition[2]-undeformedPosition[2]};
		Calc.rotationMatrixPostMultiplyOpt(units,v);
		if (workMatrix == null)
			workMatrix = new float[16];
		workMatrix[0] = units[0][0]; workMatrix[1] = units[1][0]; workMatrix[2 ] = units[2][0]; workMatrix[3 ] = 0;
		workMatrix[4] = units[0][1]; workMatrix[5] = units[1][1]; workMatrix[6 ] = units[2][1]; workMatrix[7 ] = 0;
		workMatrix[8] = units[0][2]; workMatrix[9] = units[1][2]; workMatrix[10] = units[2][2]; workMatrix[11] = 0;
		workMatrix[12] = currentPosition[0]+v[0];
		workMatrix[13] = currentPosition[1]+v[1];
		workMatrix[14] = currentPosition[2]+v[2];
		workMatrix[15] = 1;
		gl.modelviewMatrix().mult(workMatrix);
		}
	/**Transforms refPosition into this bone's current coordinate system.
	 * If the armature is transformed into world space, the returned value is also in world space.
	 * refPosition should be a location relative to the origin of the armature at rest pose.*/
	public float[] getPositionFor(float[] refPosition)
		{
		float[] v = new float[]{refPosition[0]-undeformedPosition[0],refPosition[1]-undeformedPosition[1],refPosition[2]-undeformedPosition[2]};
		Calc.rotationMatrixPostMultiplyOpt(units,v);
		v[0]+=currentPosition[0];
		v[1]+=currentPosition[1];
		v[2]+=currentPosition[2];
		return v;
		}
	/***
	 * Temporarily rotate the units matrix by degrees around the z-axis. 
	 * Note: The compute method overwrites this, so use this afterwards.
	 * Note: This does never affect children of the bone.
	 */
	public void rotateZ(float rads)
		{
		float cz = (float) Math.cos(rads);
		float sz = (float) Math.sin(rads);
		float[][] rz = new float[][]{
					{cz,-sz,0},
					{sz,cz,0},
					{0,0,1}
				};
		Calc.matrixMultiply3OptFirst(units,rz);
		}
	/***
	 * Temporarily scale the units matrix by factor along the local z-axis. 
	 * Note: The compute method overwrites this, so use this afterwards.
	 * Note: This does never affect children of the bone.
	 */
	public void scaleZ(float factor)
		{
		units[0][2] *= factor;
		units[1][2] *= factor;
		units[2][2] *= factor;
		}
	/***
	 * Temporarily move the current position matrix by value along the local axis[0-2]. 
	 * Note: The compute method overwrites this, so use this afterwards.
	 * Note: This does never affect children of the bone.
	 */
	public void moveLocal(int axis, float distance)
		{
		currentPosition[0] += units[0][axis]*distance;
		currentPosition[1] += units[1][axis]*distance;
		currentPosition[2] += units[2][axis]*distance;
		}
	
	/**
	 * This method relies on the all the parent bones, so they must be computed first.
	 * The appearance of bones in the bvh files is sorted topologically, thence the list in the Armature is also.
	 * @param frame The number of the target frame.
	 * @param interpolate The degree to which this calculation overwrites the old one.
	 */
	public void compute(final int frame, float interpolate, final boolean mirror, final boolean recursive)
		{
		if(lockInterp)
			interpolate = 1f;
		
		for(int i=0; i<3; i++)
			System.arraycopy(units[i], 0, oldUnits[i], 0, 3);
		
		float[] oldPosition = currentPosition;
		
		Bone src;
		if(mirror)
			src = mirrorBone;
		else
			src = this;
		
		int off = src.blockSize*frame;
		
		float[] add;
		if(posData)
			{
			add = new float[]{src.data[off],src.data[off+1],src.data[off+2]};
			off+=3;
			}
		else
			add = src.offsetFromParent.clone();
		
		if(rotData)
			{
			//generate units
			final float cx = (float) Math.cos(src.data[off]*toRad);
			final float sx = (float) Math.sin(src.data[off]*toRad);
			final float cy = (float) Math.cos(src.data[off+1]*toRad);
			final float sy = (float) Math.sin(src.data[off+1]*toRad) * (mirror?-1:1);
			final float cz = (float) Math.cos(src.data[off+2]*toRad);
			final float sz = (float) Math.sin(src.data[off+2]*toRad) * (mirror?-1:1);
			
			units[0][0] = cy*cz;
			units[0][1] = -cy*sz;
			units[0][2] = sy;
			units[1][0] = cx*sz + sy*cz*sx;
			units[1][1] = cx*cz - sx*sy*sz;
			units[1][2] = - sx*cy;
			units[2][0] = sx*sz - sy*cz*cx;
			units[2][1] = sx*cz + cx*sy*sz;
			units[2][2] = cx*cy;
			}
		//if(name.equals("Bone2_DUMPSCALE_ZXY"))
		//	System.out.println();
		if(hasScaleData())
			{
			float x = getXScale(frame);
			float y = getYScale(frame);
			float z = getZScale(frame);
			
			if(parent != null && parent.name.equals("Anus.Center_DUMPSCALE_XYZ"))
				{
				x/=parent.getXScale(frame);
				y/=parent.getYScale(frame);
				z/=parent.getZScale(frame);
				}
			
			units[0][0] *= x;
			units[1][0] *= x;
			units[2][0] *= x;
			units[0][1] *= y;
			units[1][1] *= y;
			units[2][1] *= y;
			units[0][2] *= z;
			units[1][2] *= z;
			units[2][2] *= z;
			}
		if (postMatrixMode == PM_EARLY)
			Calc.matrixMultiply3OptLast(postMatrix,units);
		
		
		if(mirror && mirrorPosition)
			{//Reverse x
			add[0]*=-1;
			}
		
		if(scale!=1)
			{
			for(int i=0; i<3; i++)
				{
				for(int j=0; j<3; j++)
					units[i][j]*=scale;
				}
			}
		
		if(parent==null)
			{
			//The position data replaces offset..
			currentPosition = add;
			}
		else
			{
			if(relativeUnits)
				Calc.matrixMultiply3OptLast(parent.units,units);
			
			currentPosition = Calc.add(parent.currentPosition,1,Calc.rotationMatrixPostMultiply(parent.units,add),1);
			}
		
		if (postMatrixMode == PM_LATE)
			Calc.matrixMultiply3OptLast(postMatrix,units);
		
		if(interpolate!=1)
			{
			for(int i=0; i<3; i++)
				{
				float len=0,oldLen=0;
				for(int j=0; j<3; j++)
					{
					/*oldLen+=units[i][j]*units[i][j];
					units[i][j]=units[i][j]*interpolate+oldUnits[i][j]*(1-interpolate);
					len+=units[i][j]*units[i][j];*/
					oldLen+=units[j][i]*units[j][i];
					units[j][i]=units[j][i]*interpolate+oldUnits[j][i]*(1-interpolate);
					len+=units[j][i]*units[j][i];
					}
				if(!hasScaleData())//Do not restore length when using scale data
					{
					oldLen = (float) Math.sqrt(oldLen);
					len = (float) Math.sqrt(len);
					if(len==0)
						{
						//units[i] = oldUnits[i];
						units[2][i] = oldUnits[2][i];
						units[1][i] = oldUnits[1][i];
						units[0][i] = oldUnits[0][i];//Reset if it somehow nullified
						}
					else
						{
						for(int j=0; j<3; j++)
							units[j][i]*=oldLen/len;//Normalize
						}
					}
				
				currentPosition[i]= currentPosition[i]*interpolate+oldPosition[i]*(1-interpolate);
				}
			}
		if(usePostOffset)
			{
			currentPosition[0]+=postOffset[0];
			currentPosition[1]+=postOffset[1];
			currentPosition[2]+=postOffset[2];
			}
		
		if(recursive)
			{
			Iterator<Bone> it = children.iterator();
			while(it.hasNext())
				it.next().compute(frame, interpolate, mirror, true);
			}
		}
	
	@Override
	public String toString()
		{
		return name+", ";
		}
	public void setFrameNumber(int frames)
		{
		data = new float[frames*blockSize];
		currentDataPosition=0;
		}
	public void addData(float[] data,int position)
		{
		System.arraycopy(data, position, this.data, currentDataPosition, blockSize);
		currentDataPosition+=blockSize;
		}
	/**Set the position at the frame. Must be set before computing the frame to have any effect.*/
	public void setPositionData(int frame, float x, float y, float z)
		{
		if(!posData)
			System.err.println("No location data for that bone!");
		int off = frame*blockSize;
		data[off]  = x;
		data[off+1]= y;
		data[off+2]= z;
		}
	/**Set the X/Y/Z rotation at the frame. Must be set before computing the frame to have any effect.*/
	public void setRotationData(int frame, float x, float y, float z)
		{
		if(!rotData)
			System.err.println("No rotation data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		data[off]  = x;
		data[off+1]= y;
		data[off+2]= z;
		}
	/**Set the X/Y/Z scale at the frame. Must be set before computing the frame to have any effect.
	 * Scale data is axis aligned at the rest pose!*/
	public void setScaleData(int frame, float x, float y, float z)
		{
		if(!scaData)
			System.err.println("No scale data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		if(rotData)
			off+=3;
		data[off]  = x;
		data[off+1]= y;
		data[off+2]= z;
		}
	
	public int getBlockSize()
		{
		return blockSize;
		}
	public void findMirror()
		{
		if(name.endsWith(".R"))
			mirrorBone = armature.getBone(name.substring(0,name.length()-2)+".L");
		else if(name.endsWith(".L"))
			mirrorBone = armature.getBone(name.substring(0,name.length()-2)+".R");
		else
			mirrorBone = this;
		}
	
	public float getX(int frame)
		{
		if(!posData)
			System.err.println("No location data for that bone!");
		int off = frame*blockSize;
		return data[off];
		}
	public float getY(int frame)
		{
		if(!posData)
			System.err.println("No location data for that bone!");
		int off = frame*blockSize;
		return data[off+1];
		}
	public float getZ(int frame)
		{
		if(!posData)
			System.err.println("No location data for that bone!");
		int off = frame*blockSize;
		return data[off+2];
		}
	
	public float getXRot(int frame)
		{
		if(!rotData)
			System.err.println("No rotation data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		return data[off];
		}
	public float getYRot(int frame)
		{
		if(!rotData)
			System.err.println("No rotation data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		return data[off+1];
		}
	public float getZRot(int frame)
		{
		if(!rotData)
			System.err.println("No rotation data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		return data[off+2];
		}

	public float getXScale(int frame)
		{
		if(!scaData)
			System.err.println("No scale data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		if(rotData)
			off+=3;
		return data[off];
		}
	public float getYScale(int frame)
		{
		if(!scaData)
			System.err.println("No scale data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		if(rotData)
			off+=3;
		return data[off+1];
		}
	public float getZScale(int frame)
		{
		if(!scaData)
			System.err.println("No scale data for that bone!");
		int off = frame*blockSize;
		if(posData)
			off+=3;
		if(rotData)
			off+=3;
		return data[off+2];
		}
	
	public boolean hasPosData()
		{
		return posData;
		}
	public boolean hasScaleData()
		{
		return scaData;
		}
	public void copyPostMatrix(float[][] src)
		{
		for(int i=0; i<3; i++)
			System.arraycopy(src[i], 0, postMatrix[i], 0, 3);
		}
	public void setParent(Bone bone)
		{
		parent = bone;
		bone.children.add(this);
		}
	public Bone getParent()
		{
		return parent;
		}
	/**Disable interpolation for this bone, and, optionally, its children*/
	public void lockInterp(boolean recursive)
		{
		lockInterp = true;
		if(recursive)
			{
			Iterator<Bone> it = children.iterator();
			while(it.hasNext())
				it.next().lockInterp = true;
			}
		}
	public void setEndSite(float[] endOffset)
		{
		this.endSite = endOffset;
		}
	}
